#include "StdAfx.h"
#include "BezierCurve.h"
#include "graphics/GrTransform.h"

double BezierMult(CGrVector v1, CGrVector v2)
{
	double v[4] = {
		v1.X() * v2.X(),
		v1.Y() * v2.Y(),
		v1.Z() * v2.Z(),
		v1.W() * v2.W()
	};
	return v[0] + v[1] + v[2] + v[3];
}

CBezierCurve::CBezierCurve(void)
{
	_calculated = false;

	//end point defaults
	_controlPts[0] = CGrVector(0, 0, 0);
	_controlPts[3] = CGrVector(20, 0, 20);

	//derivative point defaults
	_controlPts[1] = CGrVector(_controlPts[0].X() + double(rand())/double(RAND_MAX),
			_controlPts[0].Y() + double(rand())/double(RAND_MAX),
			_controlPts[0].Z() + double(rand())/double(RAND_MAX));
	_controlPts[2] = CGrVector(_controlPts[1].X() + double(rand())/double(RAND_MAX),
			_controlPts[1].Y() + double(rand())/double(RAND_MAX),
			_controlPts[1].Z() + double(rand())/double(RAND_MAX));
}

CBezierCurve::~CBezierCurve(void)
{
}

void CBezierCurve::setControlPoint(int n, CGrVector cp)
{
	if (n < 0 || n > 3) //get outta here
		return;

	_calculated = false;
	_controlPts[n] = cp;
}

void CBezierCurve::calculateCoefficients()
{
	CGrTransform geomat; //geometry matrix
	geomat.SetIdentity();
	geomat.M(1, 1) = geomat.M(2, 2) = 3.;
	geomat.M(1, 0) = geomat.M(3, 2) = -3.;
	geomat.M(2, 1) = -6.;
	geomat.M(2, 0) = geomat.M(3, 1) = 3.;
	geomat.M(3, 0) = -1.;

	//now we have the geometry matrix, multiply this by our control points
	//and we'll have the coefficients of our equation
	_coefficients[0] = geomat * CGrVector(_controlPts[0].X(), _controlPts[1].X(), _controlPts[2].X(), _controlPts[3].X());
	_coefficients[1] = geomat * CGrVector(_controlPts[0].Y(), _controlPts[1].Y(), _controlPts[2].Y(), _controlPts[3].Y());
	_coefficients[2] = geomat * CGrVector(_controlPts[0].Z(), _controlPts[1].Z(), _controlPts[2].Z(), _controlPts[3].Z());

	_calculated = true;
}

void CBezierCurve::debug_DrawCurve()
{
	//draw our segment
	glColor3f(1.f, 0.f, 1.f);
	glBegin(GL_LINE_STRIP);
	for (double a = 0; a < 1; a+=0.0001)
	{
		CGrVector v = assemblePolynomial(a);
		glVertex3d(BezierMult(_coefficients[0], v), 
			BezierMult(_coefficients[1], v),
			BezierMult(_coefficients[2], v));
	}
	glEnd();
}

double CBezierCurve::X(double t)
{
	if (!_calculated) calculateCoefficients();
	return BezierMult(_coefficients[0], assemblePolynomial(t));
}

double CBezierCurve::Y(double t)
{
	if (!_calculated) calculateCoefficients();
	return BezierMult(_coefficients[1], assemblePolynomial(t));
}

double CBezierCurve::Z(double t)
{
	if (!_calculated) calculateCoefficients();
	return BezierMult(_coefficients[2], assemblePolynomial(t));
}

CGrVector CBezierCurve::assemblePolynomial(double t)
{
	return CGrVector(1, t, pow(t, 2.), pow(t, 3.));
}

CBezierCurve::CBezierCurve(const CBezierCurve &bc)
{
	for (int i = 0; i < 4; i++)
		_controlPts[i] = bc._controlPts[i];
	calculateCoefficients();
}

CGrVector CBezierCurve::FindPoint(double t)
{
	return CGrVector(X(t), Y(t), Z(t), 1);
}